#include "motec_driver.hpp"
#include "devices/mcu_device.hpp"

#include <log/log.hpp>
#include <commons/sync_async/async_timer.hpp>
#include <commons/hal/canopen.hpp>
#include <commons/type/datetime.hpp>

#include <thread>
#include <math.h>

const static uint16_t IS_ENABLE_ID      = 0x2000;   // 使能      RW  uint16_t
const static uint16_t CLEAR_ERROR_ID    = 0x2005;   // 故障清除   RW  uint16_t
const static uint16_t TARGET_CURRENT_ID = 0x6071;   // 目标电流   RW  int16_t
const static uint16_t MAX_CURRENT_ID    = 0x6073;   // 最大电流   RW  uint16_t
const static uint16_t RATE_CURRENT_ID   = 0x6075;   // 额定电流   RW  uint16_t
const static uint16_t MODE_DISPLAY_ID   = 0x6061;   // 操作模式   RO  int16_t
const static uint16_t HARDWARE_VER_ID   = 0x1009;   // 硬件版本   RO  uint16_t
const static uint16_t SOFTWARE_VER_ID   = 0x100A;   // 软件版本   RO  uint16_t
const static uint16_t MAX_RPM_ID        = 0x6077;   // 电流模式下最高转速 RW uint16_t

const static int CONN_TIMEOUT_MS        = 3000;     // 通讯超时 ms
const static int CURRENT_STALL          = 2000;     // 到达2000ma可参与堵转检测

namespace sros::device::detail
{
    MotecDriver::MotecDriver(const Param &param, std::shared_ptr<McuDevice> mcu)
        : driver_impl()
        , last_can_recv_timer_(0)
        , last_conn_state_(true)
        , detect_stall_times_(0)
        , mcu_(mcu)
        , timer_(nullptr)
        , req_timer_(nullptr)
    {
        set_param(param);
    }

    void MotecDriver::init()
    {
        auto l_info = info();
        l_info.manufacturer = "北京诺信泰伺服科技有限公司";
        l_info.model = "Motec";
        l_info.datetime = "Unkown";
        set_info(l_info);

        DataUp l_data_up = {0};
        l_data_up.state = DEV_UNINIT;
        set_data_up(l_data_up);

        DataDown l_data_down = {0};
        set_data_down(l_data_down);

        last_can_recv_timer_.store(DateTime::currentDateTime().toMSecsSinceEpoch());

        if (timer_ == nullptr)
        {
            timer_ = std::make_shared<async::Timer>();
        }

        if (req_timer_ == nullptr)
        {
            req_timer_ = std::make_shared<async::Timer>();
        }
    }

    void MotecDriver::run()
    {
        auto data = data_up();
        data.state = DEV_INITING;
        set_data_up(data);

        if (timer_)
        {
            timer_->safe_start(10, 100, 0, [this](std::size_t times)
                               { on_timer_trigger_(times); });
        }

        if (req_timer_)
        {
            req_timer_->safe_start(10, 3000, 0, [this](std::size_t times)
                               { on_req_timer_trigger_(times); });
        }
    }

    void MotecDriver::close()
    {
        if (timer_)
        {
            timer_->safe_stop();
            timer_->safe_clear();
            timer_ = nullptr;
        }

        if (req_timer_)
        {
            req_timer_->safe_stop();
            req_timer_->safe_clear();
            req_timer_ = nullptr;
        }
    }

    void MotecDriver::parse(const device::proto::RspCanMsg &can_msg)
    {
        uint32_t std_id = can_msg.msg.Header.StdId;
        uint32_t base_id = std_id & 0xFF0;
        uint8_t node_id = std_id - base_id;

        auto l_param = param();
        if (node_id != l_param.id)
        {
            return;
        }

        last_can_recv_timer_.store(DateTime::currentDateTime().toMSecsSinceEpoch());

        auto data = data_up();
        data.state = DEV_RUNNING;
        set_data_up(data);

        switch (base_id)
        {
        case hal::CanOpen::RSP_STD_ID:
        {
            auto rx_data = hal::CanOpen::unpck_sdo(can_msg.msg);

            if (rx_data.cmd == hal::CanOpen::ReadRspErr)
            {
                LOG_WARN("[CAN{}] recv sdo rsp err, cmd:0x{:x}, index:0x{:x}, sub_index:0x{:x}, data:0x{:x}", 
                    l_param.can_index, rx_data.cmd, rx_data.index, rx_data.sub_index, rx_data.data);
            }
            else
            {
                switch (rx_data.index)
                {
                case IS_ENABLE_ID:
                {
                    auto data = data_up();
                    data.enable = rx_data.data == 1 ? true : false;
                    set_data_up(data);
                }
                break;
                case MAX_CURRENT_ID:
                {
                    auto data = data_up();
                    data.max_current = rx_data.data * 10;
                    set_data_up(data);
                }
                break;
                case RATE_CURRENT_ID:
                {
                    auto data = data_up();
                    data.rate_current = rx_data.data * 10;
                    set_data_up(data);
                }
                break;
                case MODE_DISPLAY_ID:
                {
                    auto data = data_up();
                    data.mode = rx_data.data;
                    set_data_up(data);
                }
                break;
                case HARDWARE_VER_ID:
                {
                    auto l_info = info();
                    l_info.hardware_version = "Ver." + std::to_string(rx_data.data);
                    set_info(l_info);
                }
                break;
                case SOFTWARE_VER_ID:
                {
                    auto l_info = info();
                    l_info.software_version = "Ver." + std::to_string(rx_data.data);
                    set_info(l_info);
                }
                break;
                case MAX_RPM_ID:
                {
                    auto l_param = param();
                    auto data = data_up();
                    data.max_rpm = rx_data.data;

                    double temp = (double)data.max_rpm * M_PI * (double)l_param.diameter;
                    temp /= (60.0f * l_param.gear_ratio);
                    data.max_spd = (int32_t)(temp + 0.5);

                    set_data_up(data);
                }
                break;
                default:
                {
                    LOG_WARN("[CAN{}] recv sdo rsp no process, cmd:0x{:x}, index:0x{:x}, sub_index:0x{:x}, data:0x{:x}", 
                        l_param.can_index, rx_data.cmd, rx_data.index, rx_data.sub_index, rx_data.data);
                }
                break;
                }
            }
        }
        break;
        case hal::CanOpen::T0_STD_ID:
        {
            uint16_t status_word = (uint16_t)(can_msg.msg.Data[0] | (can_msg.msg.Data[1] << 8));
            int16_t actual_rpm = (int16_t)(can_msg.msg.Data[2] | (can_msg.msg.Data[3] << 8));
            int16_t actual_current = (int16_t)(can_msg.msg.Data[4] | (can_msg.msg.Data[5] << 8));
            uint16_t temperature = (uint16_t)(can_msg.msg.Data[6] | (can_msg.msg.Data[7] << 8));

            auto l_param = param();
            auto data = data_up();
            data.status_word = status_word;
            data.rpm = actual_rpm;

            double temp = (double)actual_rpm * M_PI * (double)l_param.diameter;
            temp /= (60.0f * l_param.gear_ratio);
            data.spd = (int32_t)(temp + 0.5);

            data.current = actual_current * 10;
            data.temperature = temperature;
            set_data_up(data);
        }
        break;
        case hal::CanOpen::T1_STD_ID:
        {
            int32_t pluse = (int32_t)(can_msg.msg.Data[0] | (can_msg.msg.Data[1] << 8) | (can_msg.msg.Data[2] << 16) | (can_msg.msg.Data[3] << 24));
            uint32_t error = (uint32_t)(can_msg.msg.Data[4] | (can_msg.msg.Data[5] << 8) | (can_msg.msg.Data[6] << 16) | (can_msg.msg.Data[7] << 24));

            DataUp data = data_up();
            data.pulse = pluse;
            double wheel_circumference = M_PI * (double)l_param.diameter;
            double position = (pluse * wheel_circumference) / ((double)l_param.resolution * l_param.gear_ratio);
            data.pos = (int32_t)(position + 0.5);

            set_data_up(data);
        }
        break;
        case 0x700:
        {
            auto data = data_up();
            data.nmt_status = can_msg.msg.Data[0];
            set_data_up(data);
        }
        break;
        default:
        {
        }
        break;
        }
    }

    void MotecDriver::on_timer_trigger_(std::size_t times)
    {
        DataUp data = data_up();

        uint64_t current_time = DateTime::currentDateTime().toMSecsSinceEpoch();
        uint64_t time_since_last_recv = current_time - last_can_recv_timer_.load();
        bool current_conn_state = (time_since_last_recv <= CONN_TIMEOUT_MS);

        // 正常→断连
        if (!current_conn_state && last_conn_state_.load())
        {
            LOG_WARN("[MOTOR] disconnect, {} - {} = {}", current_time, last_can_recv_timer_.load(), time_since_last_recv);
            data.state = DEV_DISCONNECT;
            if (event_cb_)
            {
                event_cb_(EV_MOTOR_DISCONNECT, 0, "MOTOR disconnect");  // 触发CAN通讯断连事件
            }
        }

        last_conn_state_.store(current_conn_state);

        // 堵转
        if (abs(data.current) >= CURRENT_STALL && data.rpm == 0)
        {
            detect_stall_times_.store(detect_stall_times_.load() + 1);
            if (detect_stall_times_.load() == 3)
            {
                LOG_WARN("[MOTOR] stall");
                if (event_cb_)
                {
                    event_cb_(EV_MOTOR_STALL, 0, "MOTOR stall");    // 触发电机堵转事件
                }
            }
        }
        else
        {
            detect_stall_times_.store(0);
        }

        set_data_up(data);

        static bool need_send = true;
        // 判断 NMT状态是否处于可操作状态     
        if (data.nmt_status != 5)
        {
            // 激活整个NMT网络CAN设备
            auto tx_msg = hal::CanOpen::pck_nmt();
            auto ret = send_can_msg_(tx_msg);
            if (!ret.v)
            {
                LOG_WARN("send nmt can msg failed, {}", ret.msg);
            }
            need_send = true;
        }
        else // 可操作后 先下使能 再上使能
        {
            if (!need_send)
            {
                return;
            }

            std::vector<uint8_t> pdo_data(8, 0);
            pdo_data[0] = 0x06;

            auto tx_msg = hal::CanOpen::pck_pdo(hal::CanOpen::R0_STD_ID, pdo_data);
            auto ret = send_can_msg_(tx_msg);
            if (!ret.v)
            {
                LOG_WARN("send disable can msg failed, {}", ret.msg);
                return;
            }

            pdo_data[0] = 0x07;
            tx_msg = hal::CanOpen::pck_pdo(hal::CanOpen::R0_STD_ID, pdo_data);
            ret = send_can_msg_(tx_msg);
            if (!ret.v)
            {
                LOG_WARN("send enable1 can msg failed, {}", ret.msg);
                return;
            }

            pdo_data[0] = 0x0F;
            tx_msg = hal::CanOpen::pck_pdo(hal::CanOpen::R0_STD_ID, pdo_data);
            ret = send_can_msg_(tx_msg);
            if (!ret.v)
            {
                LOG_WARN("send enable2 can msg failed, {}", ret.msg);
                return;
            }

            need_send = false;
        }
    }

    void MotecDriver::on_req_timer_trigger_(std::size_t times)
    {
        // 查询使能状态
        auto tx_msg = hal::CanOpen::pck_sdo(hal::CanOpen::REQ_STD_ID, hal::CanOpen::ReadZeroBytes, IS_ENABLE_ID, 0, 0);
        auto ret = send_can_msg_(tx_msg);
        if (!ret.v)
        {
            LOG_WARN("send IS_ENABLE_ID can msg failed, {}", ret.msg);
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(100));

        // 查询最大电流
        tx_msg = hal::CanOpen::pck_sdo(hal::CanOpen::REQ_STD_ID, hal::CanOpen::ReadZeroBytes, MAX_CURRENT_ID, 0, 0);
        ret = send_can_msg_(tx_msg);
        if (!ret.v)
        {
            LOG_WARN("send MAX_CURRENT_ID can msg failed, {}", ret.msg);
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(100));

        // 查询额定电流
        tx_msg = hal::CanOpen::pck_sdo(hal::CanOpen::REQ_STD_ID, hal::CanOpen::ReadZeroBytes, RATE_CURRENT_ID, 0, 0);
        ret = send_can_msg_(tx_msg);
        if (!ret.v)
        {
            LOG_WARN("send RATE_CURRENT_ID can msg failed, {}", ret.msg);
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(100));

        // 查询操作模式
        tx_msg = hal::CanOpen::pck_sdo(hal::CanOpen::REQ_STD_ID, hal::CanOpen::ReadZeroBytes, MODE_DISPLAY_ID, 0, 0);
        ret = send_can_msg_(tx_msg);
        if (!ret.v)
        {
            LOG_WARN("send MODE_DISPLAY_ID can msg failed, {}", ret.msg);
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(100));

        // 查询硬件版本
        tx_msg = hal::CanOpen::pck_sdo(hal::CanOpen::REQ_STD_ID, hal::CanOpen::ReadZeroBytes, HARDWARE_VER_ID, 0, 0);
        ret = send_can_msg_(tx_msg);
        if (!ret.v)
        {
            LOG_WARN("send HARDWARE_VER_ID can msg failed, {}", ret.msg);
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(100));

        // 查询软件版本
        tx_msg = hal::CanOpen::pck_sdo(hal::CanOpen::REQ_STD_ID, hal::CanOpen::ReadZeroBytes, SOFTWARE_VER_ID, 0, 0);
        ret = send_can_msg_(tx_msg);
        if (!ret.v)
        {
            LOG_WARN("send SOFTWARE_VER_ID can msg failed, {}", ret.msg);
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(100));

        // 查询最大转速
        tx_msg = hal::CanOpen::pck_sdo(hal::CanOpen::REQ_STD_ID, hal::CanOpen::ReadZeroBytes, MAX_RPM_ID, 0, 0);
        ret = send_can_msg_(tx_msg);
        if (!ret.v)
        {
            LOG_WARN("send MAX_RPM_ID can msg failed, {}", ret.msg);
        }
    }

    RetBool MotecDriver::clear_error()
    {
        std::vector<uint8_t> pdo_data(8, 0);
        pdo_data[0] = 0x00;
        
        auto tx_msg = hal::CanOpen::pck_pdo(hal::CanOpen::R0_STD_ID, pdo_data);
        auto ret = send_can_msg_(tx_msg);
        if (!ret.v)
        {
            return ret;
        }

        pdo_data[0] = 0x80;
        tx_msg = hal::CanOpen::pck_pdo(hal::CanOpen::R0_STD_ID, pdo_data);
        ret = send_can_msg_(tx_msg);
        if (!ret.v)
        {
            return ret;
        }

        pdo_data[0] = 0x06;
        tx_msg = hal::CanOpen::pck_pdo(hal::CanOpen::R0_STD_ID, pdo_data);
        ret = send_can_msg_(tx_msg);
        if (!ret.v)
        {
            return ret;
        }

        pdo_data[0] = 0x07;
        tx_msg = hal::CanOpen::pck_pdo(hal::CanOpen::R0_STD_ID, pdo_data);
        ret = send_can_msg_(tx_msg);
        if (!ret.v)
        {
            return ret;
        }

        return ret;
    }

    RetBool MotecDriver::set_spd(const int32_t &spd)
    {
        auto l_param = param();

        int32_t spd_min = spd * 60;
        double numerator = spd_min * l_param.gear_ratio;
        double denominator = M_PI * (double)l_param.diameter;

        auto data = data_down();
        data.rpm = (int32_t)(numerator / denominator + 0.5);
        data.spd = spd;
        set_data_down(data);

        int16_t rpm = data.rpm;
        std::vector<uint8_t> pdo_data(8, 0);
        pdo_data[0] = 0x0F;
        pdo_data[1] = 0;
        pdo_data[2] = rpm;
        pdo_data[3] = rpm >> 8;
        
        auto tx_msg = hal::CanOpen::pck_pdo(hal::CanOpen::R0_STD_ID, pdo_data);
        return send_can_msg_(tx_msg);
    }

    RetBool MotecDriver::set_control(const uint16_t &control_word, const int16_t &spd)
    {
        std::vector<uint8_t> pdo_data(8, 0);
        pdo_data[0] = control_word;
        pdo_data[1] = control_word >> 8;
        pdo_data[2] = spd;
        pdo_data[3] = spd >> 8;

        auto tx_msg = hal::CanOpen::pck_pdo(hal::CanOpen::R0_STD_ID, pdo_data);
        return send_can_msg_(tx_msg);
    }

    RetBool MotecDriver::set_current(const int32_t &current)
    {
        auto l_param = param();

        auto data = data_down();
        data.current = current;
        set_data_down(data);

        int16_t l_current = current;
        std::vector<uint8_t> pdo_data(8, 0);
        pdo_data[0] = 0x0F;
        pdo_data[1] = 0;
        pdo_data[2] = 0;
        pdo_data[3] = 0;
        pdo_data[4] = l_current;
        pdo_data[5] = l_current >> 8;

        auto tx_msg = hal::CanOpen::pck_pdo(hal::CanOpen::R0_STD_ID, pdo_data);
        return send_can_msg_(tx_msg);
    }

    RetBool MotecDriver::set_enable(const bool &enable)
    {
        std::vector<uint8_t> pdo_data(8, 0);
        pdo_data[0] = enable ? 0x0F : 0x06;

        auto tx_msg = hal::CanOpen::pck_pdo(hal::CanOpen::R1_STD_ID, pdo_data);
        return send_can_msg_(tx_msg);
    }

    void MotecDriver::error_check()
    {
        // 设置电流 和 反馈电流相差过大   ->    电流超差

        // TCP未断连 其他报文有收到 NMT staus == 0x05 但是TPDO报文一定时间没收到 ->   TPDO数据丢失
    }

    RetBool MotecDriver::send_can_msg_(hal::CanTxMsg &tx_msg)
    {
        auto l_param = param();
        auto can_status_list = mcu_->cans_status();
        auto it = can_status_list.find(l_param.can_index);
        if (it == can_status_list.end())
        {
            return {false, "can not find this can index"};
        }
        if (it->second.active != 1 || it->second.error != 0)
        {
            return {false, "this can channel is not active"};
        }

        tx_msg.Header.StdId = tx_msg.Header.StdId == 0 ? tx_msg.Header.StdId : tx_msg.Header.StdId  + l_param.id;

        proto::ReqCanMsg req;
        req.index = l_param.can_index;
        req.can_msg = tx_msg;
        return mcu_->send_can_msg(req);
    }
}